#include "math.h"
#include "../lib/rtl.h"

INLINE INT Max2I(INT a, INT b){
    return a > b ? a : b;
}

INLINE INT Max3I(INT a, INT b, INT c){
    return Max2I(a, Max2I(b, c));
}

INLINE INT Min2I(INT a, INT b){
    return a < b ? a : b;
}

INLINE INT Min3I(INT a, INT b, INT c){
    return Min2I(a, Min2I(b, c));
}

INLINE INT SignI(INT x){
    return x > 0 ? 1 : (x == 0 ? 0 : -1);
}

INLINE INT Range(INT val, INT min, INT max){
    if(val <= min)
        return min;
    if(val >= max)
        return max;
    return val;
}

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wuninitialized"

INLINE FLOAT32 SqrtF(FLOAT32 x){
    FLOAT32 fRet;
    __asm__("fsqrt":"=t"(fRet):"0"(x));
    return fRet;
}

INLINE FLOAT32 SinF(FLOAT32 x){
    FLOAT32 fRet;
    __asm__("fsin":"=t"(fRet):"0"(x));
    return fRet;
}

INLINE FLOAT32 CosF(FLOAT32 x){
    FLOAT32 fRet;
    __asm__("fcos":"=t"(fRet):"0"(x));
    return fRet;
}

#pragma GCC diagnostic pop

VOID Vec2IAdd(PVECTOR2I pvOut, PVECTOR2I pvIn1, PVECTOR2I pvIn2){
    pvOut->x = pvIn1->x + pvIn2->x;
    pvOut->y = pvIn1->y + pvIn2->y;
}

VOID Vec2ISub(PVECTOR2I pvOut, PVECTOR2I pvIn1, PVECTOR2I pvIn2){
    pvOut->x = pvIn1->x - pvIn2->x;
    pvOut->y = pvIn1->y - pvIn2->y;
}

INT Vec2IDot(PVECTOR2I pvIn1, PVECTOR2I pvIn2){
    return pvIn1->x * pvIn2->x + pvIn1->y * pvIn2->y;
}

INT Vec2ICross(PVECTOR2I pvIn1, PVECTOR2I pvIn2){
    return pvIn1->x * pvIn2->y - pvIn1->y * pvIn2->x;
}

VOID Vec2ISwap(PVECTOR2I pvIn1, PVECTOR2I pvIn2){
    __xchg(pvIn1->x, pvIn2->x);
    __xchg(pvIn1->y, pvIn2->y);
}

VOID Vec2FAdd(PVECTOR2F pvOut, PVECTOR2F pvIn1, PVECTOR2F pvIn2){
    pvOut->x = pvIn1->x + pvIn2->x;
    pvOut->y = pvIn1->y + pvIn2->y;
}

VOID Vec2FSub(PVECTOR2F pvOut, PVECTOR2F pvIn1, PVECTOR2F pvIn2){
    pvOut->x = pvIn1->x - pvIn2->x;
    pvOut->y = pvIn1->y - pvIn2->y;
}

FLOAT32 Vec2FDot(PVECTOR2F pvIn1, PVECTOR2F pvIn2){
    return pvIn1->x * pvIn2->x + pvIn1->y * pvIn2->y;
}

FLOAT32 Vec2FCross(PVECTOR2F pvIn1, PVECTOR2F pvIn2){
    return pvIn1->x * pvIn2->y - pvIn1->y * pvIn2->x;
}

VOID Vec3FAdd(PVECTOR3F pvOut, PVECTOR3F pvIn1, PVECTOR3F pvIn2){
    pvOut->x = pvIn1->x + pvIn2->x;
    pvOut->y = pvIn1->y + pvIn2->y;
    pvOut->z = pvIn1->z + pvIn2->z;
}

VOID Vec3FSub(PVECTOR3F pvOut, PVECTOR3F pvIn1, PVECTOR3F pvIn2){
    pvOut->x = pvIn1->x - pvIn2->x;
    pvOut->y = pvIn1->y - pvIn2->y;
    pvOut->z = pvIn1->z - pvIn2->z;
}

VOID Vec3FMul(PVECTOR3F pvOut, PVECTOR3F pvIn1, FLOAT32 fIn2){
    pvOut->x = pvIn1->x * fIn2;
    pvOut->y = pvIn1->y * fIn2;
    pvOut->z = pvIn1->z * fIn2;
}

VOID Vec3FDiv(PVECTOR3F pvOut, PVECTOR3F pvIn1, FLOAT32 fIn2){
    fIn2 = 1.0f / fIn2;
    pvOut->x = pvIn1->x * fIn2;
    pvOut->y = pvIn1->y * fIn2;
    pvOut->z = pvIn1->z * fIn2;
}

FLOAT32 Vec3FDot(PVECTOR3F pvIn1, PVECTOR3F pvIn2){
    return pvIn1->x * pvIn2->x + pvIn1->y * pvIn2->y + pvIn1->z * pvIn2->z;
}

VOID Vec3FCross(PVECTOR3F pvOut, PVECTOR3F pvIn1, PVECTOR3F pvIn2){
    pvOut->x = pvIn1->y * pvIn2->z - pvIn1->z * pvIn2->y;
    pvOut->y = pvIn1->z * pvIn2->x - pvIn1->x * pvIn2->z;
    pvOut->z = pvIn1->x * pvIn2->y - pvIn1->y * pvIn2->x;
}

VOID Vec3FNormalize(PVECTOR3F pvVec){
    Vec3FDiv(pvVec, pvVec, Vec3FMagnitude(pvVec));
}

FLOAT32 Vec3FMagnitude(PVECTOR3F pvIn){
    return SqrtF(pvIn->x * pvIn->x + pvIn->y * pvIn->y + pvIn->z * pvIn->z);
}

FLOAT32 Vec3FSqrMagnitude(PVECTOR3F pvIn){
    return pvIn->x * pvIn->x + pvIn->y * pvIn->y + pvIn->z * pvIn->z;
}

VOID Vec3FToQuat4F(PQUAT4F pqOut, PVECTOR3F pvIn){
    pqOut->w = 0.0f;
    pqOut->x = pvIn->x;
    pqOut->y = pvIn->y;
    pqOut->z = pvIn->z;
}

VOID Quat4FMul(PQUAT4F pqOut, PQUAT4F pqIn1, PQUAT4F pqIn2){
    pqOut->w = pqIn1->w * pqIn2->w - pqIn1->x * pqIn2->x - pqIn1->y * pqIn2->y - pqIn1->z * pqIn2->z;
    pqOut->x = pqIn1->w * pqIn2->x + pqIn1->x * pqIn2->w + pqIn1->y * pqIn2->z - pqIn1->z * pqIn2->y;
    pqOut->y = pqIn1->w * pqIn2->y - pqIn1->x * pqIn2->z + pqIn1->y * pqIn2->w + pqIn1->z * pqIn2->x;
    pqOut->z = pqIn1->w * pqIn2->z + pqIn1->x * pqIn2->y - pqIn1->y * pqIn2->x + pqIn1->z * pqIn2->w;
}

VOID Quat4FMulF(PQUAT4F pqOut, PQUAT4F pqIn1, FLOAT32 fIn2){
    pqOut->w = pqIn1->w * fIn2;
    pqOut->x = pqIn1->x * fIn2;
    pqOut->y = pqIn1->y * fIn2;
    pqOut->z = pqIn1->z * fIn2;
}

VOID Quat4FDiv(PQUAT4F pqOut, PQUAT4F pqIn1, PQUAT4F pqIn2){
    QUAT4F qInv;
    RtlCopyMemory(&qInv, (LPCVOID)pqIn2, sizeof(QUAT4F));
    Quat4FInverse(&qInv);
    Quat4FMul(pqOut, pqIn1, &qInv);
}

VOID Quat4FDivF(PQUAT4F pqOut, PQUAT4F pqIn1, FLOAT32 fIn2){
    fIn2 = 1.0f / fIn2;
    pqOut->w = pqIn1->w * fIn2;
    pqOut->x = pqIn1->x * fIn2;
    pqOut->y = pqIn1->y * fIn2;
    pqOut->z = pqIn1->z * fIn2;
}

VOID Quat4FNeg(PQUAT4F pqOut, PQUAT4F pqIn){
    pqOut->w = -pqIn->w;
    pqOut->x = -pqIn->x;
    pqOut->y = -pqIn->y;
    pqOut->z = -pqIn->z;
}

VOID Quat4FMulV(PVECTOR3F pvOut, PQUAT4F pqIn1, PVECTOR3F pvIn2){
    QUAT4F qVec;
    QUAT4F qInv;
    Vec3FToQuat4F(&qVec, pvIn2);
    RtlCopyMemory(&qInv, (LPCVOID)pqIn1, sizeof(QUAT4F));
    Quat4FInverse(&qInv);
    Quat4FMul(&qVec, pqIn1, &qVec);
    Quat4FMul(&qVec, &qVec, &qInv);
    Quat4FToVec3F(pvOut, &qVec);
}

VOID Quat4FFromTo(PQUAT4F pqOut, PVECTOR3F pvFrom, PVECTOR3F pvTo){
    QUAT4F qFrom;
    QUAT4F qTo;
    Vec3FToQuat4F(&qFrom, pvFrom);
    Vec3FToQuat4F(&qTo, pvTo);
    Quat4FDiv(pqOut, &qTo, &qFrom);
}

VOID Quat4FAxisAngle(PQUAT4F pqOut, PVECTOR3F pvAxis, FLOAT32 fArc){
    FLOAT32 fSin;
    pqOut->w = CosF(fArc * 0.5f);
    fSin = SinF(fArc * 0.5f);
    pqOut->x = fSin * pvAxis->x;
    pqOut->y = fSin * pvAxis->y;
    pqOut->z = fSin * pvAxis->z;
}

VOID Quat4FEulerZXY3F(PQUAT4F pqOut, FLOAT32 fX, FLOAT32 fY, FLOAT32 fZ){
    QUAT4F qRot;
    pqOut->w = CosF(fX * 0.5f);
    pqOut->x = SinF(fX * 0.5f);
    pqOut->y = 0.0f;
    pqOut->z = 0.0f;
    RtlZeroMemory(&qRot, sizeof(QUAT4F));
    qRot.w = CosF(fY * 0.5f);
    qRot.y = SinF(fY * 0.5f);
    Quat4FMul(pqOut, pqOut, &qRot);
    qRot.w = CosF(fZ * 0.5f);
    qRot.z = SinF(fZ * 0.5f);
    qRot.y = 0.0f;
    Quat4FMul(pqOut, pqOut, &qRot);
}

VOID Quat4FEulerZXY(PQUAT4F pqOut, PVECTOR3F pvArc){
    QUAT4F qRot;
    pqOut->w = CosF(pvArc->x * 0.5f);
    pqOut->x = SinF(pvArc->x * 0.5f);
    pqOut->y = 0.0f;
    pqOut->z = 0.0f;
    RtlZeroMemory(&qRot, sizeof(QUAT4F));
    qRot.w = CosF(pvArc->y * 0.5f);
    qRot.y = SinF(pvArc->y * 0.5f);
    Quat4FMul(pqOut, pqOut, &qRot);
    qRot.w = CosF(pvArc->z * 0.5f);
    qRot.z = SinF(pvArc->z * 0.5f);
    qRot.y = 0.0f;
    Quat4FMul(pqOut, pqOut, &qRot);
}

VOID Quat4FConjugate(PQUAT4F pqIn){
    pqIn->x = -pqIn->x;
    pqIn->y = -pqIn->y;
    pqIn->z = -pqIn->z;
}

VOID Quat4FInverse(PQUAT4F pqIn){
    Quat4FConjugate(pqIn);
    Quat4FDivF(pqIn, pqIn, Quat4FSqrMagnitude(pqIn));
}

VOID Quat4FNormalize(PQUAT4F pqIn){
    Quat4FDivF(pqIn, pqIn, Quat4FMagnitude(pqIn));
}

FLOAT32 Quat4FMagnitude(PQUAT4F pqIn){
    return SqrtF(pqIn->w * pqIn->w + pqIn->x * pqIn->x + pqIn->y * pqIn->y + pqIn->z * pqIn->z);
}

FLOAT32 Quat4FSqrMagnitude(PQUAT4F pqIn){
    return pqIn->w * pqIn->w + pqIn->x * pqIn->x + pqIn->y * pqIn->y + pqIn->z * pqIn->z;
}

VOID Quat4FToVec3F(PVECTOR3F pvOut, PQUAT4F pqIn){
    pvOut->x = pqIn->x;
    pvOut->y = pqIn->y;
    pvOut->z = pqIn->z;
}